import java.io.Serializable;

/**
 * 
 * 
 * This class provides basic functionality of Monte Carlo Localization 
 * algorithm for a 1D world. All distances are measured in centimeters
 * 
 * @author Cetin Mericli
 *
 * @modifier Okan Aşık
 */

public class MCL implements Serializable
{
	//Modify these according to your settings
	static final int WORLD_SIZE = 200; //the length of the world in centimeters
	static final double DELTA = 5; // Amount of random distortion while cloning
	static final int landmarkDistance = 25;
	

	int NUM_PARTICLES; //number of particles
	
	Particle particles[]; //array for holding the particles
	Particle resampled[]; //temporary array for holding resampled particles
	Particle pose;
	Map map; //map of the environment
	
	MCL( int numParticles, Map map )
	{
		NUM_PARTICLES = numParticles;
		particles = new Particle[ NUM_PARTICLES ];
		resampled = new Particle[ NUM_PARTICLES ];
		pose = new Particle();
		
		for ( int i = 0 ; i < NUM_PARTICLES ; i++ )
		{
			particles[ i ] = new Particle();
			resampled[ i ] = new Particle();
			
		}
		this.map = map;
		reset();
	}
	
	//supply odometry information
	void motionUpdate( int movement )
	{
		for ( int i = 0; i < NUM_PARTICLES ; i++ )
		{
			particles[ i ].location += motionModel(movement);
			if ( particles[ i ].location > WORLD_SIZE )
			{
				particles[ i ].location = WORLD_SIZE;
			}
		}
	}
	
	void measurementUpdate( int sonarDistance )
	{
		for ( int i = 0; i < NUM_PARTICLES ; i++ )
		{
			particles[ i ].belief = measurementModel( sonarDistance, particles[ i ].location );
		}
		resample();
		calculatePose();
	}
	
	//according to location and sonar sensor reading return a belief value
	double measurementModel( int sensorReading, int location )
	{
		double result = 0.0;
		
		int sensorError = 5;
		
		result = calculateDistribution(sensorReading, map.expectedDistance(location));

		//if sensor reading corresponds to expected sensor reading
		/*
		if ( sensorReading >= map.expectedDistance(location)-sensorError && sensorReading <= map.expectedDistance(location)+sensorError) {
			if (location >= 0 && location < 40) {
				result = calculateDistribution(location, 20, 10);
			}
			else if (location >= 40 && location < 60) {
				result = calculateDistribution(location, 50, 5);
			}
			else if (location >= 60 && location < 110) {
				result = calculateDistribution(location, 85, 12.5);
			}
			else if (location >= 110 && location < 130) {
				result = calculateDistribution(location, 120, 5);
			}
			else if (location >= 130 && location < 150) {
				result = calculateDistribution(location, 50, 5);
			}
			else if (location >= 150 && location < 170) {
				result = calculateDistribution(location, 160, 5);
			}
			else if (location >= 170 && location < 200) {
				result = calculateDistribution(location, 185);
			}
		}
		*/
		
		System.out.println("The measurement model of sensor:"+sensorReading+" and location:" + location + " = " + result);
		
		return result*10;
	}	
	
	//the location we measured may have some error so insert that error in to the motions
	//we assume that there could be at most 1 centimeters odometry errors.
	double motionModel( int motion )
	{
		double ret = motion;
		
		if (Math.random() > 0.5) {
			ret = motion + (1 * Math.random());
		}
		else {
			ret = motion - (1 * Math.random());
		}
		
		return ret;
	}
	
	void resample()
	{
		int i;
		
		int numOfClones; //how many clones of a particle will be generated?
		double sumBelief = 0; //for normalization
		
		
		int index = 0; //particle index for temporary array
		
		//calculating the sum of the beliefs to be used in normalization
		for ( i = 0 ; i < NUM_PARTICLES ; i++ )
		{
			sumBelief += particles[ i ].belief;
		}
		
		//resample all the particles
		for ( i = 0 ; i < NUM_PARTICLES ; i++ )
		{
			numOfClones = ( int ) ( NUM_PARTICLES * particles[ i ].belief / sumBelief ); //the number of clones are proportional to the particle's belief
			if ( numOfClones > 0 ) //if the particle will be cloned
			{

				//!!! IMPORTANT !!! Modify DELTA according to your approach
				//generate a clone of the particle with a random distortion
				resampled[ index ].location = ( int )( particles[ i ].location + DELTA * Math.random() );
				
				if ( resampled[ index ].location > WORLD_SIZE )
				{
					resampled[ index ].location = WORLD_SIZE;
				}
				resampled[ index ].belief = particles[ i ].belief;
				index++;
			}
		}
		//if there is a gap between the cloned particles and the number of particles, populate random particles to fill it
		for ( i = index; i < NUM_PARTICLES ; i++ )
		{
			resampled[ i ].location = ( int )( Math.random() * WORLD_SIZE );
			resampled[ i ].belief = 1 / NUM_PARTICLES;
		}
		
		//copy the resampled particle set to the particles array
		for ( i = 0 ; i < NUM_PARTICLES ; i++ )
		{
			particles[ i ].location = resampled[ i ].location;
			particles[ i ].belief = resampled[ i ].belief;
		}
	}
	
	void reset()
	{
		//assign random locations to the particles
		for ( int i = 0; i < NUM_PARTICLES ; i++ )
		{
			particles[ i ].location = ( int ) ( Math.random() * WORLD_SIZE );
			particles[ i ].belief = 1 / NUM_PARTICLES;
		}
	}
	
	
	void calculatePose()
	{
		Particle ret = new Particle();

		double totalBelief = 0;
		double totalBeliefPosition = 0;
		for (int i=0; i<NUM_PARTICLES; i++) {
			totalBeliefPosition += particles[i].location * particles[i].belief;
			totalBelief += particles[i].belief;
		}
		
		pose.location = (int)Math.round(totalBeliefPosition / totalBelief);
		pose.belief = totalBelief / NUM_PARTICLES;
	}
	
	double calculateDistribution(double x, double mean) {
		double result = 0.0;
		double sqr_variance = 0.5;
		result = (1 / (2 * Math.sqrt(2 * Math.PI*sqr_variance))) * Math.exp(((-1.0)*(x-mean)*(x-mean))/(2*sqr_variance));
		//result = Math.exp(((-1.0)*(x-mean)*(x-mean))/(2*sqr_variance));
		return result;

	}
}
